#include "motor.h"

void motor_init()
{
	PORTC.DIRSET = PIN0_bm | PIN1_bm | PIN4_bm | PIN5_bm;
	PORTE.DIRSET = PIN0_bm | PIN1_bm;

	TCC0.CTRLA |= TC_CLKSEL_DIV8_gc;
	TCC0.CTRLB = TC_WGMODE_SS_gc;
	TCC0.PER = 0xff;
    
	TCC1.CTRLA |= TC_CLKSEL_DIV8_gc;
	TCC1.CTRLB = TC_WGMODE_SS_gc;
	TCC1.PER = 0xff;

	TCE0.CTRLA |= TC_CLKSEL_DIV8_gc;
	TCE0.CTRLB = TC_WGMODE_SS_gc;
	TCE0.PER = 0xff;
	
	TCC0.CNTL = 0;			// Stagger the motors start time to lower peak current draw
	TCC1.CNTL = 85;
	TCE0.CNTL = 170;
}

void move(uint8_t dir, uint8_t duty_cycle)
{
	TCC0_CCA = TCC0_CCB = duty_cycle;
	TCC1_CCA = TCC1_CCB = duty_cycle;
	TCE0_CCA = TCE0_CCB = duty_cycle;
	switch (dir)
	{
		case NORTH:
			TCC0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc;
			break;
		case NORTHEAST:	
			TCC0.CTRLB = TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc;
			break;
		case SOUTHEAST:
			TCC0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC_WGMODE_SS_gc;
			break;
		case SOUTH:		
			TCC0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc;
			break;
		case SOUTHWEST:
			TCC0.CTRLB = TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc;
			break;
		case NORTHWEST:
			TCC0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC_WGMODE_SS_gc;
			break;
		default:
			TCC0.CTRLB = TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC_WGMODE_SS_gc;
			break;
	}	
}	

void spin(uint8_t dir, uint8_t duty_cycle)
{
	TCC0_CCA = TCC0_CCB = duty_cycle;
	TCC1_CCA = TCC1_CCB = duty_cycle;
	TCE0_CCA = TCE0_CCB = duty_cycle;

	TCC0.CTRLB = TC_WGMODE_SS_gc;
	TCC1.CTRLB = TC_WGMODE_SS_gc;
	TCE0.CTRLB = TC_WGMODE_SS_gc;
	
	PORTC.OUTCLR = PIN0_bm | PIN1_bm | PIN4_bm | PIN5_bm;
	PORTE.OUTCLR = PIN0_bm | PIN1_bm;

	switch (dir)
	{
		case CLOCKWISE:
			TCC0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc;
			break;
		case COUNTERCLOCKWISE:
			TCC0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc;
			TCC1.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SS_gc;
			TCE0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc;
			break;
	}
}

void stop()
{
	TCC0.CTRLB = TC_WGMODE_SS_gc;
	TCC1.CTRLB = TC_WGMODE_SS_gc;
	TCE0.CTRLB = TC_WGMODE_SS_gc;
	
	PORTC.OUTCLR = PIN0_bm | PIN1_bm | PIN4_bm | PIN5_bm;
	PORTE.OUTCLR = PIN0_bm | PIN1_bm;
}
